package edu.ntnu.controller;

import java.util.List;
import edu.wsu.KheperaSimulator.RobotController;
import edu.wsu.KheperaSimulator.KSGripperStates;

/*
 * Moving across maze, record way and objects
 * 
 */
public class Collect extends State {
	
	private List<Point> balls;
	private Point light;

	public Collect(RobotController newRobot) throws NavigationException {
		super(newRobot);
		System.out.println("Starting collecting");
		balls = mapBuilder.findBalls();
		light = mapBuilder.findLight();
		
		// check whether light was found
		if (light == null) {
			throw new NavigationException("Light was not found");
		}
	}

	/**
	 * brings ball to the light
	 * @return	true if ball was brought to the light, false otherwise
	 */
	public boolean ballToLight() {
		if(robot.isObjectHeld() && driver.moveTo(light)) {
			int turned = driver.turnToLight();
			driver.moveForward(30, 1, true);
			robot.setArmState(KSGripperStates.ARM_DOWN);
			robot.sleep(50);
			robot.setGripperState(KSGripperStates.GRIP_OPEN);
			robot.sleep(50);
			robot.setArmState(KSGripperStates.ARM_UP);
			robot.sleep(50);
			robot.setGripperState(KSGripperStates.GRIP_CLOSED);
			driver.moveBackward(30, 1);
			driver.turn(turned * -1, 1);
			driver.moveBackward(30, 1);
			System.out.println("Ball was delivered");
			return true;
		}
		return false;
	}
	
	/**
	 * picks up ball
	 * @param ball	point in map where the ball is located
	 * @return		true if the ball was picked up, false otherwise
	 */
	public boolean pickUpBall(Point ball) {
		if(!robot.isObjectHeld() && driver.moveTo(ball)) {
			int turned = driver.turnToBall();
			if (turned == 0) {
				return false;
			}
			robot.setGripperState(KSGripperStates.GRIP_OPEN);
			robot.sleep(50);
			robot.setArmState(KSGripperStates.ARM_DOWN);
			robot.sleep(50);
			robot.setGripperState(KSGripperStates.GRIP_CLOSED);
			robot.sleep(50);
			robot.setArmState(KSGripperStates.ARM_UP);
			robot.sleep(50);
			driver.turn(turned * -1, 1);
			System.out.println(robot.isObjectHeld() ? "Ball was picked up" : "Ball was not picked up");
		}
		return robot.isObjectHeld();
	}
	
	@Override
	public boolean doAction() {
		if (balls.isEmpty()) {
			return true;
		}
		
		if(pickUpBall(balls.get(0))) {
			ballToLight();
		}
		
		balls.remove(0);
		return false;
	}
}
